#include <iostream>
#include <alerror/alerror.h>
#include <alproxies/almotionproxy.h>

/* for random number generation */
#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <stdlib.h>     /* srand, rand */

#include <qi/os.hpp>


class moveEmotion{
float HeadYaw, HeadPitch, LHand, RHand, HeadTime, HandTime, LShoulderRoll, LShoulderPitch, LShoulderTime,RShoulderRoll,RShoulderPitch, RShoulderTime;
unsigned int desiredJoints;//From rightmost to leftmost: bit 1: head, bit 2: shoulder, bit 3: hand

float genRand(float maxFloat);



public:
	moveEmotion(void):HeadYaw(0), HeadPitch(0), LHand(0), RHand(0), HeadTime(0), HandTime(0), LShoulderRoll(0), LShoulderPitch(0), LShoulderTime(0),RShoulderRoll(0),RShoulderPitch(0), RShoulderTime(0), desiredJoints(0){}
int moveAffectiveHead(float valence, float arousal);


/* NAO's hand can be completely open (1) to completely closed (0).
  *
  * Mapping rules between (valence, arousal) to nao's hand movement:
  *  - Valence : the greater the valence is, the more open the hand is.
  *
  *  - Arousal : inversely mapped with execution time of each designated hand's movement,
  *  generally, the faster the movement is, the higher the arousal is.
  *
  *
  *valence compris between -1 and 1
  * arousal compris between 0 and 1
  */
int moveAffectiveHand(float valence, float arousal);


/*
  *nao's LShoulder (H25, V4.0) can move between:
  * Pitch : 2.0857 (back) to -2.0857 (front)
  * Roll  : 1.3265 (outward) to -0.3142 (inward)
  *
  * Mapping rule for LShoulder's movement and (valence, arousal):
  *  - Valence (-1, 1) => (1.5, 0) LShoulderPitch : the greater the valence is, the higher the arm is.
  *  - Arousal (0, 1)  => (-0.15,1.15) LShoulderYaw: the greater the arousal, the more ouverture the left arm makes.
  *  - Arousal (0,)  => execution time: the higher the arousal is, the faster the movement is.
  *
  *nao's RShoulder (H25, V4.0) can move between:
  * Pitch : 2.0857 (back) to -2.0857 (front)
  * Roll  : -1.3265 (outward) to 0.3142 (inward)
  *
  * Mapping rule for RShoulder's movement and (valence, arousal)
  *  - Valence (-1, 1) => (1.5, 0) RShoulderPitch : the greater the valence is, the higher the arm is.
  *  - Arousal (0, 1)  => (0.15, -1.5) RShoulderYaw : the greater the arousal is, the more ouverture the right arm makes
  *  - Arousal (0,1)  => execution time: the higher the arousal is, the faster the movement is.
  *
  */

int moveAffectiveShoulder(float valence, float arousal);


/*
  *
  * Execute the motion in speed portion mode, better to map between Arousal and Speed.
  *
  *
  */
int moveRobotWithSpeed(AL::ALMotionProxy motion, float speedPortion);

//===============================================================
/*
  * For debug only.
  *
  * Sadness  : valence = (-1, -0.5), arousal = (0, 0.25), speedPortion = 0.05
  *
  * Happiness: ++head, hand++ : valence = (0.3, 0.6), arousal = (0.3, 1.0), speedPortion = 0.12
  *            ++shoulder++   : valence = (-0.4, 0.5), arousal = (0.3, 1.0), speedPortion = 0.12
  *
  * Anger: ++head, hand++ : valence = (-1.0, 1.0), arousal = (0.3, 1.0), speedPortion = 0.6
  *        ++shoulder++   : valence = (-0.4, 0.5), arousal = (0.3, 1.0), speedPortion = 0.6
  *
  * Serenity: ++head++             : valence = (-0.3, 0.7), arousal = (0.0, 0.25), speedPortion = 0.1
  *           ++hand, shoulder++   : valence = (-0.5, 0.5), arousal = (0.0, 0.25), speedPortion = 0.1
  *
  */
//===============================================================
/*
  * Intensity is between (0, 1). The higher the intensity, the sadder the expression should be.
  * This can be expressed by the HeadPitch, higher Headpitch is higher intensity; and in motion speed,
  * the higher the intensity, the slower the motion is.
  */
void moveSadness(AL::ALMotionProxy motion, float intensity,float maxTime = 10);

/*
  * Happiness: ++head, hand++ : valence = (0.3, 0.6), arousal = (0.3, 1.0), speedPortion = 0.12
  *            ++shoulder++   : valence = (-0.4, 0.5), arousal = (0.3, 1.0), speedPortion = 0.12
  *            intensity affects 30% of the speed of the motion. The higher the intensity, the faster the motion is.
  *
*/
void moveHappiness(AL::ALMotionProxy motion,float intensity, float maxTime = 10);

void moveAnger(AL::ALMotionProxy motion, float intensity, float maxTime = 10);

void moveSerenity(AL::ALMotionProxy motion,float intensity, float maxTime = 10);

/*
 * From rightmost to leftmost: bit 1: head, bit 2: shoulder, bit 3: hand
 * For example: joint = 1 means head only
 *              joint = 2 means shoulder only
 *              joint = 3 means head + shoulder
 *              joint = 4 means hand only
 *              joint = 5 means head + hand
 *              joint = 6 means shoulder + hand
 *              joint = 7 means haed + shoulder + hand
 */
int setDesiredJoints(int joint);

};